
#include <dlfcn.h>
#include <math.h>
#include <algorithm>
#include <boost/format.hpp>
#include <opencv2/opencv.hpp>

#include "gammaDict.hpp"

#include <iostream>
#include <fstream>

#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/thread.hpp>
#define BOOST_DATE_TIME_SOURCE



using namespace std;
using namespace cv;




boost::posix_time::ptime time_now,time_now1;
boost::posix_time::millisec_posix_time_system_config::time_duration_type time_elapse;

const float BGR2Z_Tab_[] =
{
	0,		0.000326797,		0.000653595,		0.000980392,		0.00130719,		0.00163399,		0.00196078,		0.00228758,		0.00261438,		0.00294118,
	0.00326797,		0.00334654,		0.00367651,		0.00402472,		0.00439144,		0.00477695,		0.00518152,		0.00560539,		0.00604883,		0.00651209,
	0.00699541,		0.00749903,		0.00802319,		0.00856813,		0.00913406,		0.00972122,		0.0103298,		0.0109601,		0.0116122,		0.0122865,
	0.012983,		0.0137021,		0.0144438,		0.0152085,		0.0159963,		0.0168074,		0.017642,		0.0185002,		0.0193824,		0.0202886,
	0.021219,		0.0221739,		0.0231534,		0.0241576,		0.0251869,		0.0262412,		0.0273209,		0.028426,		0.0295568,		0.0307134,
	0.031896,		0.0331048,		0.0343398,		0.0356013,		0.0368895,		0.0382044,		0.0395462,		0.0409152,		0.0423114,		0.043735,
	0.0451862,		0.0466651,		0.0481718,		0.0497066,		0.0512695,		0.0528606,		0.0544803,		0.0561285,		0.0578054,		0.0595112,
	0.061246,		0.06301,		0.0648033,		0.0666259,		0.0684782,		0.0703601,		0.0722719,		0.0742136,		0.0761854,		0.0781874,
	0.0802198,		0.0822827,		0.0843762,		0.0865005,		0.0886556,		0.0908417,		0.093059,		0.0953075,		0.0975874,		0.0998987,
	0.102242,		0.104616,		0.107023,		0.109462,		0.111932,		0.114435,		0.116971,		0.119538,		0.122139,		0.124772,
	0.127438,		0.130136,		0.132868,		0.135633,		0.138432,		0.141263,		0.144128,		0.147027,		0.14996,		0.152926,
	0.155926,		0.158961,		0.162029,		0.165132,		0.168269,		0.171441,		0.174647,		0.177888,		0.181164,		0.184475,
	0.187821,		0.191202,		0.194618,		0.198069,		0.201556,		0.205079,		0.208637,		0.212231,		0.215861,		0.219526,
	0.223228,		0.226966,		0.23074,		0.234551,		0.238398,		0.242281,		0.246201,		0.250158,		0.254152,		0.258183,
	0.262251,		0.266356,		0.270498,		0.274677,		0.278894,		0.283149,		0.287441,		0.291771,		0.296138,		0.300544,
	0.304987,		0.309469,		0.313989,		0.318547,		0.323143,		0.327778,		0.332452,		0.337164,		0.341914,		0.346704,
	0.351533,		0.3564,		0.361307,		0.366253,		0.371238,		0.376262,		0.381326,		0.386429,		0.391573,		0.396755,
	0.401978,		0.40724,		0.412543,		0.417885,		0.423268,		0.42869,		0.434154,		0.439657,		0.445201,		0.450786,
	0.456411,		0.462077,		0.467784,		0.473532,		0.47932,		0.48515,		0.491021,		0.496933,		0.502886,		0.508881,
	0.514918,		0.520996,		0.527115,		0.533277,		0.53948,		0.545725,		0.552012,		0.55834,		0.564712,		0.571125,
	0.577581,		0.584079,		0.590619,		0.597202,		0.603827,		0.610496,		0.617207,		0.62396,		0.630757,		0.637597,
	0.64448,		0.651406,		0.658375,		0.665387,		0.672443,		0.679543,		0.686686,		0.693872,		0.701102,		0.708376,
	0.715694,		0.723055,		0.730461,		0.737911,		0.745404,		0.752942,		0.760525,		0.768151,		0.775822,		0.783538,
	0.791298,		0.799103,		0.806952,		0.814847,		0.822786,		0.83077,		0.838799,		0.846873,		0.854993,		0.863157,
	0.871367,		0.879623,		0.887923,		0.89627,		0.904661,		0.913099,		0.921582,		0.930111,		0.938686,		0.947307,
	0.955974,		0.964686,		0.973445,		0.982251,		0.991102,		1
};

const float BGR_Uniform_Tab_[] =
{
	0,		0.00392157,		0.00784314,		0.0117647,		0.0156863,		0.0196078,		0.0235294,		0.027451,		0.0313726,		0.0352941,
	0.0392157,		0.0431373,		0.0470588,		0.0509804,		0.054902,		0.0588235,		0.0627451,		0.0666667,		0.0705882,		0.0745098,
	0.0784314,		0.0823529,		0.0862745,		0.0901961,		0.0941177,		0.0980392,		0.101961,		0.105882,		0.109804,		0.113725,
	0.117647,		0.121569,		0.12549,		0.129412,		0.133333,		0.137255,		0.141176,		0.145098,		0.14902,		0.152941,
	0.156863,		0.160784,		0.164706,		0.168627,		0.172549,		0.176471,		0.180392,		0.184314,		0.188235,		0.192157,
	0.196078,		0.2,		0.203922,		0.207843,		0.211765,		0.215686,		0.219608,		0.223529,		0.227451,		0.231373,
	0.235294,		0.239216,		0.243137,		0.247059,		0.25098,		0.254902,		0.258824,		0.262745,		0.266667,		0.270588,
	0.27451,		0.278431,		0.282353,		0.286275,		0.290196,		0.294118,		0.298039,		0.301961,		0.305882,		0.309804,
	0.313726,		0.317647,		0.321569,		0.32549,		0.329412,		0.333333,		0.337255,		0.341176,		0.345098,		0.34902,
	0.352941,		0.356863,		0.360784,		0.364706,		0.368627,		0.372549,		0.376471,		0.380392,		0.384314,		0.388235,
	0.392157,		0.396078,		0.4,		0.403922,		0.407843,		0.411765,		0.415686,		0.419608,		0.423529,		0.427451,
	0.431373,		0.435294,		0.439216,		0.443137,		0.447059,		0.45098,		0.454902,		0.458824,		0.462745,		0.466667,
	0.470588,		0.47451,		0.478431,		0.482353,		0.486275,		0.490196,		0.494118,		0.498039,		0.501961,		0.505882,
	0.509804,		0.513726,		0.517647,		0.521569,		0.52549,		0.529412,		0.533333,		0.537255,		0.541176,		0.545098,
	0.54902,		0.552941,		0.556863,		0.560784,		0.564706,		0.568627,		0.572549,		0.576471,		0.580392,		0.584314,
	0.588235,		0.592157,		0.596078,		0.6,		0.603922,		0.607843,		0.611765,		0.615686,		0.619608,		0.623529,
	0.627451,		0.631373,		0.635294,		0.639216,		0.643137,		0.647059,		0.65098,		0.654902,		0.658824,		0.662745,
	0.666667,		0.670588,		0.67451,		0.678431,		0.682353,		0.686275,		0.690196,		0.694118,		0.698039,		0.701961,
	0.705882,		0.709804,		0.713726,		0.717647,		0.721569,		0.72549,		0.729412,		0.733333,		0.737255,		0.741176,
	0.745098,		0.74902,		0.752941,		0.756863,		0.760784,		0.764706,		0.768628,		0.772549,		0.776471,		0.780392,
	0.784314,		0.788235,		0.792157,		0.796079,		0.8,		0.803922,		0.807843,		0.811765,		0.815686,		0.819608,
	0.823529,		0.827451,		0.831373,		0.835294,		0.839216,		0.843137,		0.847059,		0.85098,		0.854902,		0.858824,
	0.862745,		0.866667,		0.870588,		0.87451,		0.878431,		0.882353,		0.886275,		0.890196,		0.894118,		0.898039,
	0.901961,		0.905882,		0.909804,		0.913726,		0.917647,		0.921569,		0.92549,		0.929412,		0.933333,		0.937255,
	0.941177,		0.945098,		0.94902,		0.952941,		0.956863,		0.960784,		0.964706,		0.968628,		0.972549,		0.976471,
	0.980392,		0.984314,		0.988235,		0.992157,		0.996078,		1
};

//refers to the OpenCv official answer，define a class for parallel data processors
class BGR2Z_Loop_Invoker : public ParallelLoopBody
{
public:
	BGR2Z_Loop_Invoker(const Mat& _src, Mat& _dst):ParallelLoopBody(), src(_src), dst(_dst) //class constructor
	{

	}
	void operator()(const Range& range) const //overload operator ()
	{
		//get the total element number of a row. This is equal to cols*channels or step1
		int r,g,b;
		float tabR, tabG, tabB;
		float Z,gray,diff;
		uchar* yS;
		float* yD;
		int shift;
		for( int rowIdx = range.start; rowIdx < range.end; ++rowIdx)
		{
			yS = (uchar*)src.ptr<uchar>(rowIdx);
			yD = (float*)dst.ptr<float>(rowIdx);
			for (int colIdx = 0; colIdx < src.cols; colIdx++)
			{
				shift = 3 * colIdx;
				int use_table_flag = 1;
				if(use_table_flag == 0)
				{
					//ver 1: calculate Z directly
					float uniformS = 1.0/255.0;
					float smallS = 1.0/12.0;
					float largeAdder = 0.055/1.055;
					float largeS = 1.0/1.055;
					float r,g,b;
					r = float(yS[shift+2]) * uniformS;
					g = float(yS[shift+1]) * uniformS;
					b = float(yS[shift]) * uniformS;

					if (r <= 0.04045)
					{
						r = r * smallS;
					}
					else
					{
						//r = exp2(2.4 * log2(r * largeS + largeAdder));
						r = r * largeS + largeAdder;
						r = r * r;
						if (g <= 0.04045)
						{
							g = g * smallS;
						}
						else
						{
							//g = exp2(2.4 * log2(g * largeS + largeAdder));
							g = g * largeS + largeAdder;
							g = g * g;
							if (b <= 0.04045)
							{
								b = b * smallS;
							}
							else
							{
								//b = exp2(2.4 * log2(b * largeS + largeAdder));
								b = b * largeS + largeAdder;
								b = b * b;
							}
						}
					}
					Z = 0.019334 * r + 0.119193 * g + 0.950527 * b;
				}
				else
				{
					//ver 2: use look up table to get Z
					r = yS[shift+2];
					g = yS[shift+1];
					b = yS[shift];
					tabR = BGR2Z_Tab_[r];
					if(r > 10)
					{
						tabG = BGR2Z_Tab_[g];
						if(g > 10)
						{
							tabB = BGR2Z_Tab_[b];
						}
						else
						{
							tabB = BGR_Uniform_Tab_[b];
						}
					}
					else
					{
						tabG = BGR_Uniform_Tab_[g];
						tabB = BGR_Uniform_Tab_[b];
					}
					Z = 0.019334 * tabR + 0.119193 * tabG + 0.950527 * tabB;
				}

//				//BGR2Gray
//				gray = 0.2989 * BGR_Uniform_Tab_[r] +
//					   0.5870 * BGR_Uniform_Tab_[g] +
//					   0.1140 * BGR_Uniform_Tab_[b];
//
//				//calculate difference image
//				diff = abs(Z - gray);
//				yD[colIdx] = diff;
				yD[colIdx] = Z ;
			}
		}
	}
//	void operator()(const Range& range) const //overload operator（）
//	{
//		//get the total element number of a row. This is equal to cols*channels or step1
//		int stepSrc = (int)(src.step/src.elemSize1());
//		int stepDst = (int)(dst.step/dst.elemSize1());
//		Vec3f intensity;
//		float r,g,b,Z;
//		for (int colIdx = range.start; colIdx < range.end; ++colIdx)
//		{
//			uchar* pData = (uchar*)src.col(colIdx).data;
//			float* pDstData = (float*)dst.col(colIdx).data;
//			for (int i = 0; i < src.rows; ++i)
//			{
//				//pData[i*stepSrc] = std::pow(pData[i*stepSrc],3);
//				int shift = i*stepSrc;
//				r = float(pData[shift+2]) ;
//				g = float(pData[shift+1]) ;
//				b = float(pData[shift]) ;
//
////				printf("x:%d y:%d r:%f g:%f b:%f Z:%f\n",colIdx,i,r,g,b,Z);
//				if (r <= 10.0)
//				{
//					r = r / (255.0*12.0) ;
//					g = g / (255.0) ;
//					b = b / (255.0) ;
//
//				}
//				else
//				{
//					r = gmad::dict::query((int)r) ;
//
//					if (g <= 10.0)
//					{
//						g = g / (255.0 * 12.0) ;
//						b = b / (255.0) ;
//					}
//					else
//					{
//						g = gmad::dict::query((int)g) ;
//						if (b <= 10.0)
//						{
//							b = b / (255.0 * 12.0);
//						}
//						else
//						{
//							b = gmad::dict::query((int)b) ;
//						}
//					}
//				}
//
//				Z = 0.019334 * r + 0.119193 * g + 0.950527 * b;
////				printf("b:%f g:%f r:%f Z:%f\n",b,g,r,Z);
//				//dst.at<float>(i, colIdx) = (float) Z;
//				pDstData[i*stepDst] = (float) Z;
//			}
//		}
//	}
public:
	const Mat src;
	Mat dst;
};




// using Mat_ iterator
void colorReduce8(cv::Mat &image, int div=64) {
      // get iterators
      cv::Mat_<cv::Vec3b>::iterator it= image.begin<cv::Vec3b>();
      cv::Mat_<cv::Vec3b>::iterator itend= image.end<cv::Vec3b>();
      for ( ; it!= itend; ++it) {

    	 printf("%d %d %d \n",(*it)[0],(*it)[1],(*it)[2]);
//        (*it)[0]= (*it)[0]/div*div + div/2;
//        (*it)[1]= (*it)[1]/div*div + div/2;
//        (*it)[2]= (*it)[2]/div*div + div/2;
      }
}


int main(int argc,char** argv)
{

	gmad::dict::initGamma();

	Mat frameloc = imread("013.jpg", CV_LOAD_IMAGE_UNCHANGED);
	if(frameloc.empty())
	{
		cout << "图像加载失败！"	<< endl;
		//system("pause");
		return 	-1;
	}
//    int w = 0;
//    for (int v = frameloc.rows - 1; v >= 0; v--)
//    {
//        for (int u = 0; u <frameloc.cols; u++)
//        {
//        	frameloc.at<cv::Vec3b>(v, u)[0] = 255;
//        	frameloc.at<cv::Vec3b>(v, u)[1] = 255;
//        	frameloc.at<cv::Vec3b>(v, u)[2] = 255;
//        }
//    }
//	imshow("src", frameloc);

	cv::Mat Z = Mat::zeros(frameloc.rows, frameloc.cols, CV_32FC1);
    int totalCols = frameloc.cols;

    cv::Mat srcDouble ;

	time_now = boost::posix_time::microsec_clock::universal_time();




    int32_t parfor_flag = 0;
    if(parfor_flag == 1)
    {
    	BGR2Z_Loop_Invoker BGR2Z_Loop_body(frameloc,Z);
    //	parallel_for_(Range(0, totalCols), BGR2Z_Loop_body);
    	parallel_for_(Range(0, frameloc.rows), BGR2Z_Loop_body);
    }
    else
    {
//    	CountBaseAndLast(t0,"FROM XYZ");
    	cv::Mat srcDouble;
        frameloc.convertTo(srcDouble, CV_32FC3, 1/255.0);
        cv::Mat frmElement = cv::Mat(srcDouble.rows, srcDouble.cols, CV_32FC3, Scalar::all(0.055));
        frmElement = (srcDouble + frmElement)/1.055;
        cv::Mat frmXYZ = frmElement.mul(frmElement);
        std::vector<cv::Mat> XYZplanes;
        cv::split(frmXYZ, XYZplanes);
        cv::Mat Z = XYZplanes[0] * 0.013929122 + XYZplanes[1] * 0.097097002 + XYZplanes[2] * 0.71418547;
//        CountBaseAndLast(t0,"FROM XYZ");
//        cv::Mat I_gray;
//        cv::cvtColor(srcDouble, I_gray, CV_BGR2GRAY);
        //I_diff = abs(Z - I_gray);

		imshow("dist 32", Z);
		waitKey(0);

    }

    time_now1 = boost::posix_time::microsec_clock::universal_time();
	time_elapse = time_now1 - time_now;
	int ticks = time_elapse.ticks();
	std::cout<<"microsec_clock: for Converting RGB2XYZ: "<<ticks<<" microsecs"<<endl;


//    cv::Mat_<cv::Vec3b> cimage= BGR2Z_Loop_body.dst;
//    cv::Mat_<cv::Vec3b>::iterator it=cimage.begin();
//    cv::Mat_<cv::Vec3b>::iterator itend=cimage.end();
//    for ( ; it!= itend; it++) {
//    	printf("%f %f %f \n",
//      (*it)[0],
//      (*it)[1],
//      (*it)[2]);
//    }
//	colorReduce8(BGR2Z_Loop_body.dst,64);

//	cout<<BGR2Z_Loop_body.dst.row(0)<<endl;


//    cv::Mat dst_gray(BGR2Z_Loop_body.dst.rows,BGR2Z_Loop_body.dst.cols, CV_8UC1);
//    memset(dst_gray.data,255,BGR2Z_Loop_body.dst.rows*BGR2Z_Loop_body.dst.cols);
//
//	float *dst_data_ptr = (float*)BGR2Z_Loop_body.dst.data;
//	uchar *dst_gray_data_ptr = dst_gray.data ;
//
//    for(int i=0;i<BGR2Z_Loop_body.dst.rows*BGR2Z_Loop_body.dst.cols;i++)
//    {
//    	*dst_gray_data_ptr = (uchar)((*dst_data_ptr/1.089054)*255);
////    	printf("%f %d \n",*dst_data_ptr,*dst_gray_data_ptr);
//    	dst_data_ptr ++ ;
//    	dst_gray_data_ptr ++ ;
//    }
//    imwrite("dst_gray_CPU.jpg",dst_gray);
//
//
//    cv::Mat dst2(BGR2Z_Loop_body.dst.rows,BGR2Z_Loop_body.dst.cols, CV_8UC1);
//    cvConvertScale(&(BGR2Z_Loop_body.dst),&dst2);


//	destroyWindow("MyWindow");

	return 0;
}


